#include "RMRManipulator.h"
/*
 * RMRManipulator.cpp
 * 
 * Definition of RMRManipulator class
 * 
 * Manages manipulator code
 */


// constructor
RMRManipulator::RMRManipulator(Joystick *joy) :

shoot_extend(SOLENOID, SHOOTER_EXTEND),
shoot_retract(SOLENOID, SHOOTER_RETRACT),
door_extend(SOLENOID, DOOR_EXTEND),
door_retract(SOLENOID, DOOR_RETRACT),
ball_switch(new ButtonFromDigitalInput(SIDECAR_1, BALL_COUNTER)),
//fire_button(new ButtonFromDriverStation(SHOOT_BUTTON)),
fire_button(new ButtonFromJoystick(joy, SHOOT_BUTTON_JOY)),
//test_fire_button(new ButtonFromDriverStation(SHOOT_BUTTON)),
left_conveyor_motor(SIDECAR_1, LEFT_CONVEYOR_CHANNEL),
right_conveyor_motor(SIDECAR_2 /*SIDECAR_1*/, RIGHT_CONVEYOR_CHANNEL),
compressor_relay(SIDECAR_2, COMPRESSOR_RELAY, Relay::kForwardOnly),
max_pressure_switch(new ButtonFromDigitalInput(SIDECAR_1, MAX_PRESSURE)),
conveyor_fwd_switch(new ButtonFromJoystick(joy, CONVEYOR_FWD_JOY)),
conveyor_rvs_switch(new ButtonFromJoystick(joy, CONVEYOR_RVS_JOY)),
joystick(joy)

{
	// setup ball switch
	ball_switch.SetNeededSamples(BALL_SWITCH_TIME);
	ball_switch.Enable();
	
	// setup fire button
	fire_button.SetOnSamples(FIRE_BUTTON_ON_SAMPLES);
	fire_button.SetOffSamples(FIRE_BUTTON_OFF_SAMPLES);
	fire_button.Enable();

}

// initialize anything we need to (besides stuff we do in the constructor)
void RMRManipulator::InitManipulator()
{
	
}

void RMRManipulator::TestButton()
{
	
}

// call this method every so often to handle the interaction between the manipulator controls and the manipulator hardware
void RMRManipulator::doManipulator()
{
	static int i = 0;
	static int period = DEBUG_OUT_PERIOD;
	static bool conveyor_on = false;
	static bool compressor_on = false;
	
	// do manipulator stuff, like read buttons on the driver station and turn on pneumatics, flash lights
	
	// we need a copy of the DriverStation object to get button presses from it
	//DriverStation * ds = DriverStation::GetInstance();
	
	
	
	//turn compressor on if we need more pressure
	if (max_pressure_switch.Get())
	{
		compressor_on = false;
		compressor_relay.Set(Relay::kOff);	
	}
	else
	{
		compressor_on = true;
		compressor_relay.Set(Relay::kOn);
	}
	
	
	
	// shoot when button is pressed, rapid-fire
	if (fire_button.Get())
	{
		shoot_extend.Set(true);
		shoot_retract.Set(false);
		SetRIOUserLED(1);
	}
	else
	{
		shoot_extend.Set(false);
		shoot_retract.Set(true);
		SetRIOUserLED(0);
	}
	
	
	
	// turn on (forward and reverse) and off the conveyor motor
	if (conveyor_fwd_switch.Get())
	{
		conveyor_on = true;
		left_conveyor_motor.Set(CONVEYOR_FWD_SPEED);
		right_conveyor_motor.Set(CONVEYOR_FWD_SPEED);
	}
	else if(conveyor_rvs_switch.Get())
	{
		conveyor_on = true;
		left_conveyor_motor.Set(CONVEYOR_RVS_SPEED);
		right_conveyor_motor.Set(CONVEYOR_RVS_SPEED);
	}
	/*else if(conveyor_rvs_slow_switch.Get())
	{
		conveyor_on = true;
		left_conveyor_motor.Set(CONVEYOR_RVS_SLOW_SPEED);
		right_conveyor_motor.Set(CONVEYOR_RVS_SLOW_SPEED);
	}*/
	else
	{
		conveyor_on = false;
		left_conveyor_motor.Set(0);
		right_conveyor_motor.Set(0);
	}
	
	/*
	if (ds->GetDigitalIn(DOOR_OVERRIDE_BUTTON))
	{
		door_extend.Set(true);
		door_retract.Set(false);
	}
	else
	{
		// lower the door whenever the ball switch has been pressed for a long amount of time
		if (ball_switch.Get())
		{
			door_extend.Set(true);
			door_retract.Set(false);
		}
		// raise the door whenever the ball switch is not pressed
		else
		{
			door_extend.Set(false);
			door_retract.Set(true);
		}
	}
	*/
	
	
	// debug out stuff
	i++;
	if (i > period)
	{
		i = 0;
		printf("conveyor %s\n", conveyor_on ? "on" : "off");
		printf("conveyor left = %f, right = %f\n", left_conveyor_motor.Get(), right_conveyor_motor.Get());
		printf("compressor %s\n", compressor_on ? "on" : "off");
//		printf("ball switch counter period = %.2f, status = %s\n", ball_switch_counter_period, ball_switch.Get() ? "on" : "off");
	}
}

//here be dragons
//Mouth.Insert(Food.Get());
//printf("%s is hungry", Mouth.Owner->Name);
//printf("as long as the thingymahizit doesn't break we will be fine" \n);

// here dont be dragons

